#include <Arduino.h>
#include <WiFi.h>
#include <XboxSeriesXControllerESP32_asukiaaa.hpp>

int PWMPin = 9; //2~13

const int freq = 400; //频率400hz
const int pwmChannel = 0;//使用0通道
const int resolution = 10;//分辨率8位也就是0-255

// Required to replace with your xbox address
// 需要在此替换成自己的手柄蓝牙MAC地址
XboxSeriesXControllerESP32_asukiaaa::Core
    xboxController("40:8e:2c:39:45:7d");

String xbox_string()
{
  String str = String(xboxController.xboxNotif.btnY) + "," +
               String(xboxController.xboxNotif.btnX) + "," +
               String(xboxController.xboxNotif.btnB) + "," +
               String(xboxController.xboxNotif.btnA) + "," +
               String(xboxController.xboxNotif.btnLB) + "," +
               String(xboxController.xboxNotif.btnRB) + "," +
               String(xboxController.xboxNotif.btnSelect) + "," +
               String(xboxController.xboxNotif.btnStart) + "," +
               String(xboxController.xboxNotif.btnXbox) + "," +
               String(xboxController.xboxNotif.btnShare) + "," +
               String(xboxController.xboxNotif.btnLS) + "," +
               String(xboxController.xboxNotif.btnRS) + "," +
               String(xboxController.xboxNotif.btnDirUp) + "," +
               String(xboxController.xboxNotif.btnDirRight) + "," +
               String(xboxController.xboxNotif.btnDirDown) + "," +
               String(xboxController.xboxNotif.btnDirLeft) + "," +
               String(xboxController.xboxNotif.joyLHori) + "," +
               String(xboxController.xboxNotif.joyLVert) + "," +
               String(xboxController.xboxNotif.joyRHori) + "," +
               String(xboxController.xboxNotif.joyRVert) + "," +
               String(xboxController.xboxNotif.trigLT) + "," +
               String(xboxController.xboxNotif.trigRT) + "\n";
  return str;
};

static int gear = 0; // Gear range is 0-5
static int gearUpperLast = 0;
static int gearDownLast = 0;
static int speed = 0;

static int obtainThrottle(void)
{
  if (!(gearDownLast && xboxController.xboxNotif.btnA))
  {
    gearDownLast = xboxController.xboxNotif.btnA;
    if (gear > 0 && xboxController.xboxNotif.btnA)
    {
      gear--;
    }
  }

  if (!(gearUpperLast && xboxController.xboxNotif.btnY))
  {
    gearUpperLast = xboxController.xboxNotif.btnY;
    if (gear < 5 && xboxController.xboxNotif.btnY)
    {
      gear++;
    }
  }

  return (400 + gear * 100 + xboxController.xboxNotif.trigRT / 10);

}

void setup() {
  Serial.begin(115200);  // 初始化串口通信，波特率设置为115200，你可根据实际情况调整

  xboxController.begin();

  while(Serial.read()>=0){} // clear serial port's buffer

  pinMode(PWMPin, OUTPUT);

  // 配置PWM功能
  ledcSetup(pwmChannel, freq, resolution);
  
  // 关联PWM与管脚
  ledcAttachPin(PWMPin, pwmChannel);
}

String comStr = "";
int comInt=0;
int pwnvalue = 0;

void loop() {
  xboxController.onLoop();
  if (xboxController.isConnected())
  {
    if (xboxController.isWaitingForFirstNotification())
    {
      Serial.println("waiting for first notification");
    }
    else
    {
      ledcWrite(pwmChannel, obtainThrottle());
    }
  }
  else
  {
    if (xboxController.getCountFailedConnection() > 2)
    {
      ESP.restart();
    }
  }

  delay(100);

#if 0
  if (Serial.available()) {
    delay(10);
    comStr = Serial.readString(); // read out the string
    comInt = comStr.toInt(); // convert the string to integer
    comInt = constrain(comInt, 0, 1000); // limit the integer between to 1000 and 2000

    printf("The comInt is %d\n", comInt);
    if (comInt <= 1000 && comInt >= 0)
    {
      ledcWrite(pwmChannel, comInt);
    }
  }
#endif
}